/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package com.kristou.samples;

import com.kristou.urgLibJ.Connection.EthernetConnection;
import com.kristou.urgLibJ.RangeSensor.Capture.CaptureData;
import com.kristou.urgLibJ.RangeSensor.Capture.CaptureSettings;
import com.kristou.urgLibJ.RangeSensor.RangeSensorInformation;
import com.kristou.urgLibJ.RangeSensor.UrgDevice;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.SocketException;
import java.util.ArrayList;
import java.util.Vector;

/**
 *
 * @author kristou
 */
public class EthernetSampleProgram {
    
    public static double wide = 800;
    public static double high = 600;
    
    public static double ang1 = Math.atan(2 * high / wide) * 180 / Math.PI;
    public static double ang2 = Math.atan(0.5 * wide / high) * 180 / Math.PI;
    
    /*           0                   1080
                   .                .
                     .     |      .
                       .   |    .
                         . |  .
180 -----------------------|---------------------------900
                       .   |   .                 |
                  .        |        .            |h
             .             |              .      |
         .                 |        w/2        . |
        -------------------|----------------------540+4arctan(w/(2h))
   180+4arctan(2h/w)      540
    */
    static Vector GetValidSteps(CaptureData data){
        int i = 180;
        if(data.steps.size() < 1080){
            System.out.println("not a whole capture");
            return null;
        }
        
        Vector result = new Vector();
                
        for(i = 180 ; i < 180 + 4 * ang1;i ++ ){
            double border = 0.5 * wide / Math.cos((0.25 * i - 45) * Math.PI / 180  );
            if(data.steps.get(i).distances.get(0) > 10 && data.steps.get(i).distances.get(0) < border ){
                result.add(i);
            }
        }
        
        for(i = (int) (180 + 4 * ang1 + 1); i < 540 ;i ++ ){
            double border = high / Math.cos((135 - 0.25 * i ) * Math.PI / 180 );
            if(data.steps.get(i).distances.get(0) > 10 && data.steps.get(i).distances.get(0) < border ){
                result.add(i);
            }
        }
    
        for(i = 540; i < 540 + 4 * ang2 ;i ++ ){
            double border = high / Math.cos((0.25 * i - 135 ) * Math.PI / 180);
            if(data.steps.get(i).distances.get(0) > 10 && data.steps.get(i).distances.get(0) < border ){
                result.add(i);
            }
        }
        
        for(i =  (int) (540 + 4 * ang2 + 1) ; i < 900 ; i ++ ){
            double border = 0.5 * wide / Math.cos((225 - 0.25 * i ) *  Math.PI / 180);
            if(data.steps.get(i).distances.get(0) > 10 && data.steps.get(i).distances.get(0) < border ){
                result.add(i);
            }
        }
        return result;
    }
    
    static ArrayList<PointGroup> GetPointGroups(Vector validSteps){
        if(validSteps == null){
            return null;
        }
        ArrayList<PointGroup> result = new ArrayList<PointGroup>();
        
        int size = validSteps.size();
        int index = 0;
        int j = 0;
        for (int i = 0 ; i < size - 1; i ++ ) {
            if((int)(validSteps.get(i + 1))  == (int)(validSteps.get(i)) + 1){
                continue;
            } else {
                PointGroup pg = new PointGroup(index,i - j + 1,(int)validSteps.get(j));  //if not coninutional points, we consider as different group.
                result.add(pg);
                j = i + 1;
                index ++;
            }
        }
        if(result.size() >= 1 ){
            return result;
        } else {
            return null;
        }
    }
    
    static Point GetCenterPoint(PointGroup pg,CaptureData data){ 
        Long adis;   //average distance
        double aang; //average angle
        double x;    // center point x 
        double y;    //center point y
        
        if(pg == null || data == null){
            return null;
        }
        
        aang = Math.abs(pg.first /4  + pg.length / 8 - 45.125) ;   //( pg.first + (pg.length - 1) / 2  - 180 ) / 4
        
        Long sum = 0L;
        for(int i = 0; i < pg.length; i ++ ){
            sum += data.steps.get(i + pg.first).distances.get(0);
        }
        adis = sum / pg.length ;
        
        System.out.println("aang = " + aang + "; adis = " + adis);
        
        x = wide /2 - adis * Math.cos(aang *  Math.PI / 180);
        y = Math.abs(adis * Math.sin(aang *  Math.PI / 180));
        
        if( x < 0 ){
            return null;
        }        
        return new Point(x,y);
    }

    public static void main(String[] args) throws SocketException {
        //Create an UrgDevice with the ethernet connection
        UrgDevice device = new UrgDevice(new EthernetConnection());
        int times = 20000;
        
        DatagramSocket socket = null;
        InetAddress address = null;
        int port = 0;

        // Connect to the sensor
        if (device.connect("192.168.0.10")) {
            System.out.println("connected");

            //Get the sensor information
            RangeSensorInformation info = device.getInformation();
            if(info != null){
            System.out.println("Sensor model:" + info.product);
            System.out.println("Sensor serial number:" + info.serial_number);
            }else{
                System.out.println("Sensor error:" + device.what());
            }

            //Set the continuous capture type, Please refer to the SCIP protocol for further details
            device.setCaptureMode(CaptureSettings.CaptureMode.MD_Capture_mode);
            try {
                socket = new DatagramSocket(8800);
                byte[] udpdata = new byte[128];
                DatagramPacket packet = new DatagramPacket(udpdata,udpdata.length);
                socket.receive(packet);
                
                System.out.println("server get " + new String(udpdata,0,packet.getLength()) + " from client");
                
                address = packet.getAddress();
                port = packet.getPort();
                
            } catch(Exception e){
            
            }
            //We set the capture type to a continuous mode so we have to start the capture
            device.startCapture();

            for (int i = 0; i < times; i++) {
                //Data reception happens when calling capture
                CaptureData data = device.capture();
                if(data != null) {
                    System.out.println("Scan " + (i + 1) + ", stesps " + data.steps.size());
                } else {
                    System.out.println("Sensor error:" + device.what());
                }
                
                Vector validSteps;
                validSteps = GetValidSteps(data);
                
                if(validSteps != null){
                    /*for(int k =0;k<validSteps.size();k++){
                        System.out.println("validSteps:" + validSteps.get(k) + " distance = " + data.steps.get(k).distances.get(0));
                    }*/
                    System.out.println("validSteps:" + validSteps);
                } else {
                    System.out.println("validSteps is null");
                }
                if(validSteps != null){
                    ArrayList<PointGroup> apg = GetPointGroups(validSteps);
                    if(apg != null){
                        for(int j = 0;j < apg.size();j ++){
                           System.out.println("Point Group index = " + apg.get(j).index + " : length = " + apg.get(j).length);
                           Point cp = GetCenterPoint(apg.get(j),data);
                           if(cp != null){
                               System.out.println("Center Point of index = " + apg.get(j).index + " x = " + cp.x + " y = " + cp.y);
                               try {
                                   byte[] udata = (" x = " + cp.x + " y = " + cp.y).getBytes();
                                   DatagramPacket packet2 = new DatagramPacket(udata,udata.length,address,port);
                                   socket.send(packet2);
                               } catch(Exception e){
                                   
                               }
                           } else {
                               System.out.println("Center Point of index = " + apg.get(j).index + " is null");
                           }
                        }
                    } else {
                        System.out.println("Point Groupes is null");
                    }
                } 
            }

            socket.close();
            //Stop the cature
            device.stopCapture();

            //Disconnect from the sensor
            device.disconnect();

        } else {
            System.out.println("not connected: " + device.what());
        }
    }
}
